#include "utils.hpp"

#ifdef SHOW_RESULT
#include "camera.h"
Vector3 Vec3d_to_Vec3(Vector3d vec) {
    return (Vector3){(float)vec._x, (float)vec._y, (float)vec._z};
}

void Draw_Circles(Vector3d *vecs) {
    for (size_t i = 0; i < 4; i++)
        DrawLine3D(Vec3d_to_Vec3(vecs[i]), Vec3d_to_Vec3(vecs[i+1]), WHITE);
    DrawLine3D(Vec3d_to_Vec3(vecs[4]), Vec3d_to_Vec3(vecs[0]), WHITE);
    // Vector3d point1, point2;
    // for (double i = 0; i < 1; i+=0.05) {
    //     point1 = i*vecs[1] + (1-i)*vecs[2];
    //     point2 = i*vecs[4] + (1-i)*vecs[3];
    //     DrawLine3D(Vec3d_to_Vec3(point1), Vec3d_to_Vec3(point2), WHITE);
    // }
    // for (double i = 0; i < 1; i+=0.05) {
    //     point1 = i*vecs[1] + (1-i)*vecs[4];
    //     point2 = i*vecs[2] + (1-i)*vecs[3];
    //     DrawLine3D(Vec3d_to_Vec3(point1), Vec3d_to_Vec3(point2), WHITE);
    // }
}

void Draw_Body(Mat rotation) {
    constexpr float LENGTH = 1;
    constexpr float WIDTH = 1;
    constexpr float HEIGHT = 2;
    Vector3d veca[5], vecb[5];
    veca[0] = rotation * Vector3d(0, WIDTH, 1.5*HEIGHT);
    veca[1] = rotation * Vector3d(-LENGTH, WIDTH, HEIGHT);
    veca[2] = rotation * Vector3d(-LENGTH, WIDTH, -HEIGHT);
    veca[3] = rotation * Vector3d(LENGTH, WIDTH, -HEIGHT);
    veca[4] = rotation * Vector3d(LENGTH, WIDTH, HEIGHT);
    vecb[0] = rotation * Vector3d(0, -WIDTH, 1.5*HEIGHT);
    vecb[1] = rotation * Vector3d(-LENGTH, -WIDTH, HEIGHT);
    vecb[2] = rotation * Vector3d(-LENGTH, -WIDTH, -HEIGHT);
    vecb[3] = rotation * Vector3d(LENGTH, -WIDTH, -HEIGHT);
    vecb[4] = rotation * Vector3d(LENGTH, -WIDTH, HEIGHT);
    Vector3d vecex = rotation * Vector3d(1, 0, 0);
    Vector3d vecey = rotation * Vector3d(0, 1, 0);
    Vector3d vecez = rotation * Vector3d(0, 0, 1);
    Draw_Circles(veca);
    Draw_Circles(vecb);
    for (uint32_t i = 0; i < 5; i++)
        DrawLine3D(Vec3d_to_Vec3(veca[i]), Vec3d_to_Vec3(vecb[i]), WHITE);    
    DrawLine3D((Vector3){0, 0, 0}, Vec3d_to_Vec3(vecex), GOLD);
    DrawLine3D((Vector3){0, 0, 0}, Vec3d_to_Vec3(vecey), GOLD);
    DrawLine3D((Vector3){0, 0, 0}, Vec3d_to_Vec3(vecez), YELLOW);
}
#endif
